#ifndef __EMB_PQUEUE_H
#define __EMB_PQUEUE_H

#include "emb.h"
#include <string.h>


typedef int8_t (*emb_pqueu_cmp_fn_t)(const uint8_t* First, const uint8_t* Second);


typedef struct  {
    uint8_t* buf;           
    emb_pqueu_cmp_fn_t cmp; 
    uint16_t esize;         
    uint16_t total;         
    uint16_t count;         
} emb_pqueue_ctrl_t;


int emb_pqueue_init(emb_pqueue_ctrl_t* pqueue, uint8_t* buf, 
                   uint16_t buf_size, uint16_t esize, 
                   emb_pqueu_cmp_fn_t cmp) 
{
    pqueue->buf = buf;
    pqueue->cmp = cmp;
    pqueue->esize = esize;
    pqueue->total = buf_size / esize; 
    pqueue->count = 0;
    
    return 0;
}


static inline uint16_t emb_pqueue_size(emb_pqueue_ctrl_t* pqueue) 
{
    return pqueue->count;
}


int emb_pqueue_push(emb_pqueue_ctrl_t* pqueue, const uint8_t* src) 
{
    if(emb_pqueue_size(pqueue)>=pqueue->total)
    {
        return -1;
    }

    uint16_t child = pqueue->count++;
    uint16_t parent = (child - 1) / 2;

    while (child > 0) {
        const uint8_t* parent_ptr = pqueue->buf + parent * pqueue->esize;

        if (pqueue->cmp(src, parent_ptr) >= 0) {
            break;
        }

        memcpy(pqueue->buf + child * pqueue->esize, 
              parent_ptr, 
              pqueue->esize);
        child = parent;
        parent = (child - 1) / 2;
    }

    memcpy(pqueue->buf + child * pqueue->esize, src, pqueue->esize);
    return 0;
}


int emb_pqueue_pop(emb_pqueue_ctrl_t* pqueue, uint8_t* dest) 
{
    if (emb_pqueue_size(pqueue)==0) {
        return -1;
    }

    uint8_t *parent_ptr,*child_ptr,*left,*right;
    uint16_t parent = 0,child;
    uint8_t *last = pqueue->buf + (pqueue->count) * pqueue->esize;

    memcpy(dest, pqueue->buf, pqueue->esize);   
    pqueue->count--;


    while (1) {
        child = 2 * parent + 1; 
        if (child >= pqueue->count) break;


        if (child + 1 < pqueue->count) {
            left = pqueue->buf + child * pqueue->esize;
            right = left + pqueue->esize;
            if (pqueue->cmp(left, right) > 0) {
                child++;
            }
        }


        parent_ptr = pqueue->buf + parent * pqueue->esize;
        child_ptr = pqueue->buf + child * pqueue->esize;

        if (pqueue->cmp(child_ptr, last) >= 0)
        {
            memcpy(parent_ptr, child_ptr, pqueue->esize);
            parent = child;
        }
        else{
            break;
        }
    }

    memcpy(parent_ptr, last, pqueue->esize);
    return 0;
}


static void* emb_pqueue_head(emb_pqueue_ctrl_t* pqueue) 
{
    if(emb_pqueue_size(pqueue)!=0)
    {
        return pqueue->buf;
    }

    return NULL;
}


static inline void emb_pqueue_flush(emb_pqueue_ctrl_t* pqueue)
 {
    pqueue->count = 0;
}

#endif 